#include "check_node.h"

#include <px4_platform_common/getopt.h>
#include <px4_platform_common/log.h>
#include <px4_platform_common/posix.h>

#include <uORB/topics/parameter_update.h>
#include <stdlib.h>
int CheckNode::print_status()
{
    PX4_INFO("Running");
    // TODO: print additional runtime information about the state of the module

    return 0;
}

int CheckNode::custom_command(int argc, char *argv[])
{
    /*
    if (!is_running()) {
        print_usage("not running");
        return 1;
    }

    // additional custom commands can be handled like this:
    if (!strcmp(argv[0], "do-something")) {
        get_instance()->do_something();
        return 0;
    }
     */

    return print_usage("unknown command");
}


int CheckNode::task_spawn(int argc, char *argv[])
{
    _task_id = px4_task_spawn_cmd("module",
                                  SCHED_DEFAULT,
                                  SCHED_PRIORITY_DEFAULT,
                                  1024,
                                  (px4_main_t)&run_trampoline,
                                  (char *const *)argv);

    if (_task_id < 0)
    {
        _task_id = -1;
        return -errno;
    }

    return 0;
}

CheckNode *CheckNode::instantiate(int argc, char *argv[])
{
    int example_param = 0;
    bool example_flag = false;
    bool error_flag = false;

    int myoptind = 1;
    int ch;
    const char *myoptarg = nullptr;

    // parse CLI arguments
    while ((ch = px4_getopt(argc, argv, "p:f", &myoptind, &myoptarg)) != EOF)
    {
        switch (ch)
        {
            case 'p':
                example_param = (int)strtol(myoptarg, nullptr, 10);
                break;

            case 'f':
                example_flag = true;
                break;

            case '?':
                error_flag = true;
                break;

            default:
                PX4_WARN("unrecognized flag");
                error_flag = true;
                break;
        }
    }

    if (error_flag)
    {
        return nullptr;
    }

    CheckNode *instance = new CheckNode(example_param, example_flag);

    if (instance == nullptr)
    {
        PX4_ERR("alloc failed");
    }

    return instance;
}

CheckNode::CheckNode(int example_param, bool example_flag)
    : ModuleParams(nullptr)
{
}
int CheckNode::check_process(int n)
{
	int num[4];
	num[0] = n / 1000;//千数
	num[1] = n / 100 % 10;//百数
	num[2] = n / 10 % 10;//十数
	num[3] = n % 10;//个位
	for(int i = 0;i < 4;i++)
	{
	num[i] = (num[i] + 9) % 10;//加9，然后除以10 取余
	}
	//第1 位和第3 位上的数字互换，第2 位和第4 位上的数字互换
	int check_value = num[0] * 10  + num[1];
	return check_value;

}
int flag=0;

// void CheckNode::generateRandomNumber() {
//     // 创建随机数生成器
// frand1
// }



void CheckNode::run()
{
while(1)
{

for (auto &telemetry_status :  _telemetry_status_subs) {
		telemetry_status_s telemetry;
telemetry.heartbeat_type_onboard_controller=false;
telemetry.heartbeat_type_gcs=false;
		telemetry_status.copy(&telemetry);

if(telemetry.heartbeat_type_onboard_controller)
{
     flag=10;
// PX4_INFO("onboard");

while(rand==0)
{
    // PX4_INFO("random()=%ld",random());
    rand= (int)(random()%9000 + 1000);
     	time_tick=hrt_absolute_time();
 	// PX4_INFO("time_tick=%lld",time_tick);
    usleep(100000);
}
// PX4_INFO("rand=%d",rand);
   _transponder_report.hor_velocity=rand;
   _transponder_report_pub.publish(_transponder_report);
}

if(telemetry.heartbeat_type_gcs)
{
    PX4_INFO("gcs");
         flag=11;
// PX4_INFO("onboard");

while(randgcs==0)
{
    // PX4_INFO("random()=%ld",random());
    randgcs= (int)(random()%9000 + 1000);
     	time_tick_gcs=hrt_absolute_time();
         if(randgcs!=0)
         {
             break;
         }
 	// PX4_INFO("time_tick=%lld",time_tick);
    usleep(100000);
}
//  PX4_INFO("randgcs=%d",randgcs);
 a05_gcscheck_s _a05_gcscheck1;
   _a05_gcscheck.tx=randgcs;
    _a05_gcscheck_sub.copy(&_a05_gcscheck1);
    _a05_gcscheck.rx=_a05_gcscheck1.rx;
    if(check_process(randgcs)==(int)_a05_gcscheck.rx)
    {
check_gcspass=true;
    }
    
   _a05_gcscheck_pub.publish(_a05_gcscheck);
  
    // PX4_INFO("_a05_gcscheck.rx=%ld",_a05_gcscheck.rx);
    //  PX4_INFO("_a05_gcscheck.tx=%ld",_a05_gcscheck.tx);
    // PX4_INFO("check_process(_a05_gcscheck.tx)=%d",check_process(_a05_gcscheck.tx));
}


_a04_check_sub.copy(&_a04_check);
// PX4_INFO("_a04_check.state1=%d",_a04_check.state1);
// PX4_INFO("_a04_check.state2=%d",_a04_check.state2);
// PX4_INFO("_a04_check.check=%d",_a04_check.check);

//  PX4_INFO("check_process(rand)=%d",check_process(rand));
 if((flag==10)&&(check_process(rand)!=(int)_a04_check.check)&&(hrt_absolute_time()-time_tick)>10000000)
   {
   _a04_check.state2=5;
   //PX4_INFO("check not pass");
   }
   else if((flag==11)&&!check_gcspass&&(hrt_absolute_time()-time_tick_gcs)>20000000)
   {
        _a04_check.state2=5;
        // PX4_INFO("gcs check not pass");
   }
   else
   {
   _a04_check.state2=0;
  //  PX4_INFO("check  pass");
   }
   _a04_check_pub.publish(_a04_check);

// if(telemetry.heartbeat_type_gcs)
// {
// PX4_INFO("gcs");
// }

}

       usleep(100000);

// _a04_check_sub.copy(&_a04_check);
//    PX4_INFO("_a04_check.state1=%d",_a04_check.state1);
//    if(_a04_check.state1==1)
//    {
//     PX4_INFO("flag=%d",flag);
// //  if(rand==0)
// //  {
// //  	rand= random()%9999;
// //  	PX4_INFO("rand=%d",rand);
// //  	time_tick=hrt_absolute_time();
// //  	PX4_INFO("time_tick=%lld",time_tick);
// // }
// PX4_INFO("rand=%d",rand);
//    _transponder_report.hor_velocity=rand;
//    _transponder_report_pub.publish(_transponder_report);
//    }
//     PX4_INFO("check_process(rand)=%d",check_process(rand));
//      PX4_INFO("_a04_check.check=%d",(int)_a04_check.check);
//    if((check_process(rand)==(int)_a04_check.check)&&(hrt_absolute_time()-time_tick)<10000000)
//    {
//    _a04_check.state2=1;
//    PX4_INFO("dsadsadas");
//    }
//    if((check_process(rand)!=(int)_a04_check.check)&&(hrt_absolute_time()-time_tick)>10000000)
//    {
//     _a04_check.state2=2;
//     }
//     PX4_INFO("_a04_check.state2=%d",_a04_check.state2);

        }


}

void CheckNode::parameters_update(bool force)
{
    // check for parameter updates
    if (_parameter_update_sub.updated() || force)
    {
        // clear update
        parameter_update_s update;
        _parameter_update_sub.copy(&update);

        // update parameters from storage
        updateParams();
    }
}

int CheckNode::print_usage(const char *reason)
{
    if (reason)
    {
        PX4_WARN("%s\n", reason);
    }

    PRINT_MODULE_DESCRIPTION(
        R"DESCR_STR(
### Description
Section that describes the provided module functionality.

This is a template for a module running as a task in the background with start/stop/status functionality.

### Implementation
Section describing the high-level implementation of this module.

### Examples
CLI usage example:
$ module start -f -p 42

)DESCR_STR");

    PRINT_MODULE_USAGE_NAME("module", "template");
    PRINT_MODULE_USAGE_COMMAND("start");
    PRINT_MODULE_USAGE_PARAM_FLAG('f', "Optional example flag", true);
    PRINT_MODULE_USAGE_PARAM_INT('p', 0, 0, 1000, "Optional example parameter", true);
    PRINT_MODULE_USAGE_DEFAULT_COMMANDS();

    return 0;
}

int check_node_main(int argc, char *argv[])
{
    return CheckNode::main(argc, argv);
}


